
#ifndef SLAM_ROS_SLAM_ROS_OCCUPANCY_GRID_NODE_H
#define SLAM_ROS_SLAM_ROS_OCCUPANCY_GRID_NODE_H

#include <cmath>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cairo/cairo.h"
#include "slam/common/port.h"
#include "slam/io/image.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/id.h"
#include "slam/transform/rigid_transform.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/node_constants.h"
#include "slam_ros/ros_log_sink.h"
#include "slam_ros/submap.h"
#include "slam_ros_msgs/msg/submap_list.hpp"
#include "slam_ros_msgs/srv/submap_query.hpp"
#include "gflags/gflags.h"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include <rclcpp/rclcpp.hpp>

namespace slam_ros {

using ::slam::io::PaintSubmapSlicesResult;
using ::slam::io::SubmapSlice;
using ::slam::mapping::SubmapId;

class OccupancyGridNode : public rclcpp::Node 
{
 public:
  explicit OccupancyGridNode(
      const double resolution, 
      const double publish_period_sec, 
      const std::string map_topic_name,
      const bool include_frozen_submaps,
      const bool include_unfrozen_submaps);
  ~OccupancyGridNode() {}

  OccupancyGridNode(const OccupancyGridNode&) = delete;
  OccupancyGridNode& operator=(const OccupancyGridNode&) = delete;

 private:
  void HandleSubmapList(const typename slam_ros_msgs::msg::SubmapList::SharedPtr msg);
  void DrawAndPublish(void);

  const double resolution_;
  const std::string map_topic_name_;
  const bool include_frozen_submaps_;
  const bool include_unfrozen_submaps_;

  absl::Mutex mutex_;
  ::rclcpp::Client<slam_ros_msgs::srv::SubmapQuery>::SharedPtr client_ GUARDED_BY(mutex_);
  ::rclcpp::Subscription<slam_ros_msgs::msg::SubmapList>::SharedPtr submap_list_subscriber_ GUARDED_BY(mutex_);
  ::rclcpp::Publisher<::nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ GUARDED_BY(mutex_);
  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
  ::rclcpp::TimerBase::SharedPtr occupancy_grid_publisher_timer_;
  std::string last_frame_id_;
  ::rclcpp::Time last_timestamp_;
  int slam_type_ = -1;
};

}

#endif